/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : triangulate_factor.h
 *@brief   : triangulate factor
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/13
 *
 */
#ifndef TRIANGULATE_FACTOR_H_
#define TRIANGULATE_FACTOR_H_

#include <ceres/ceres.h>
#include "common/type.h"

namespace optimizer {

// 相机内参结构（对于归一化点，焦距和主点已归一化）
struct CameraIntrinsics {
    double fx = 1.0;  // 归一化后为1
    double fy = 1.0;
    double cx = 0.0;  // 归一化后为主点偏移为0
    double cy = 0.0;
};

// 相机位姿结构（旋转和平移）
struct CameraPose {
    Eigen::Quaterniond rotation;  // 从世界到相机的旋转
    Eigen::Vector3d translation;  // 从世界到相机的平移
};

// 重投影误差代价函数
struct ReprojectionErrorCostFunctor {
    ReprojectionErrorCostFunctor(double observed_x, double observed_y,
                                const CameraPose& pose, const Eigen::Matrix4d& extrinsic_matrix,
                                const CameraIntrinsics& intrinsics)
        : observed_x_(observed_x), observed_y_(observed_y),
          pose_(pose), extrinsic_matrix_(extrinsic_matrix), intrinsics_(intrinsics) {}

    template <typename T>
    bool operator()(const T* const world_point, T* residuals) const {
        // 世界点坐标 [X, Y, Z]，扩展为齐次坐标 [X, Y, Z, 1]
        Eigen::Matrix<T, 4, 1> point_world;
        point_world << T(world_point[0]), T(world_point[1]), T(world_point[2]), T(1.0);
    
        // 1. 构建车辆在世界坐标系下的变换矩阵 Twb（世界→车辆）
        Eigen::Matrix<T, 4, 4> Twb = Eigen::Matrix<T, 4, 4>::Identity();
        Eigen::Quaternion<T> quat_vehicle(
            T(pose_.rotation.w()), T(pose_.rotation.x()),
            T(pose_.rotation.y()), T(pose_.rotation.z())
        );
    
        // 修正：使用非模板形式的 block 操作（行, 列, 行数, 列数）
        Twb.block(0, 0, 3, 3) = quat_vehicle.toRotationMatrix();  // 车辆旋转（世界→车辆）
    
        // 修正：使用非模板形式的 block 并显式构造向量
        Twb.block(0, 3, 3, 1) = Eigen::Matrix<T, 3, 1>(
            T(pose_.translation.x()), 
            T(pose_.translation.y()), 
            T(pose_.translation.z())
        );
    
        // 2. 相机在车辆坐标系下的外参矩阵 Tbc（车辆→相机）
        Eigen::Matrix<T, 4, 4> Tbc = extrinsic_matrix_.cast<T>();
    
        // 3. 计算世界→相机的变换矩阵 Twc = Twb * Tbc（世界→车辆→相机）
        Eigen::Matrix<T, 4, 4> Twc = Twb * Tbc;
        Eigen::Matrix<T, 4, 4> Tcw = Twc.inverse();
        // 4. 世界点转换到相机坐标系（使用 Twc 直接变换）
        Eigen::Matrix<T, 4, 1> point_cam_homogeneous = Twc * point_world;
    
        // 5. 检查深度是否有效（避免除零）
        T z_cam = point_cam_homogeneous[2];
        if (z_cam <= T(1e-6)) {  // 深度接近零或为负，无效
            return false;
        }
    
        // 6. 投影到归一化平面（x/z, y/z）
        T x = point_cam_homogeneous[0] / z_cam;
        T y = point_cam_homogeneous[1] / z_cam;
    
        // 7. 内参均为1，预测值即为归一化坐标
        T predicted_x = x;
        T predicted_y = y;
    
        // 8. 计算残差（观测值 - 预测值，观测值为归一化坐标）
        residuals[0] = observed_x_ - predicted_x;
        residuals[1] = observed_y_ - predicted_y;
    
        return true;
    }

    static ceres::CostFunction* Create(double observed_x, double observed_y,
                                      const CameraPose& pose, const Eigen::Matrix4d& extrinsic_matrix, 
                                      const CameraIntrinsics& intrinsics) {
        return new ceres::AutoDiffCostFunction<
            ReprojectionErrorCostFunctor, 2, 3>(  // 2个残差，3个优化变量
            new ReprojectionErrorCostFunctor(observed_x, observed_y, pose, extrinsic_matrix, intrinsics)
        );
    }

private:
    const double observed_x_;
    const double observed_y_;
    const CameraPose pose_;
    const Eigen::Matrix4d extrinsic_matrix_;
    const CameraIntrinsics intrinsics_;
};


class TriangulateFactor {
public:
    TriangulateFactor(const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix);
    bool OptimizeWorldPoints(
        const std::vector<Eigen::Vector2d>& normalized_points,  // 归一化图像点
        const std::vector<Eigen::Matrix4d>& world_poses,             // 相机位姿（固定）
        Eigen::Vector3d& world_point                            // 待优化的世界点
    );
private:
    const Eigen::Matrix3d& intrinsic_matrix_;
    const Eigen::Matrix4d& extrinsic_matrix_;
};

}
#endif